#pragma once
#include <Common/headers.h>
#include <Utility/limit.h>
#include <Geometry/line.h>
#include <Geometry/cubic_bspline_curve_cuda.h>

namespace shermit {
namespace fitting {
namespace curve {
	template<typename Float_t>
	class DistTk {
	public:
		Float_t dist;
		Float_t tk;
		GENERAL DistTk() {
			dist = NumericLimit<Float_t>::max();
			tk = dist;
		}
		GENERAL DistTk(Float_t d, Float_t t) {
			dist = d;
			tk = t;
		}

		__device__ void merge(const DistTk<Float_t>& rhs) {
			if (rhs.dist < dist) {
				dist = rhs.dist;
				tk = rhs.tk;
			}
		}
	};

	// sample range [tl, tr) on the entire curve
	template<typename Float_t>
	__global__ void sampleCubicBSplineCurveGlobal(
		CubicBSplineCurve<Float_t, DeviceType::GPU> curve,
		Float_t step, Float_t tl, Float_t tr,
		Float_t* tsamples, nv::vec2<Float_t>* p_tsamples,
		nv::vec2<Float_t>* tangents,
		nv::vec2<Float_t>* normals
	)
	{
		int idx = blockIdx.x * blockDim.x + threadIdx.x;
		Float_t t = tl + step * idx;
		if (t >= tr) return;

		tsamples[idx] = t;
		p_tsamples[idx] = curve.at(t);
		tangents[idx] = curve.tangent(t);
		normals[idx] = curve.normal(t);
	}


	// sample local ranges (tl[i], tr[i]) on the curve
	template<typename Float_t>
	__global__ void sampleCubicBSplineCurveLinesLocal(
		CubicBSplineCurve<Float_t, DeviceType::GPU> curve,
		int spr, const bool* found, 
		const Float_t* tl, const Float_t* tr,
		Float_t* tsamples, nv::vec2<Float_t>* p_tsamples,
		nv::vec2<Float_t>* tangents,
		nv::vec2<Float_t>* normals
	)
	{
		int pt_idx = blockIdx.x;
		if (found[pt_idx]) return;
		int offset = threadIdx.x;
		Float_t step = (tr[pt_idx] - tl[pt_idx]) / spr;
		Float_t t = tl[pt_idx] + step * offset;
		if (offset >= spr) return;

		int it = pt_idx * spr + offset;
		tsamples[it] = t;
		p_tsamples[it] = curve.at(t);
		tangents[it] = curve.tangent(t);
		normals[it] = curve.normal(t);
	}

	template<typename Float_t>
	__global__ void pickClosestTksDirect(
		CubicBSplineCurve<Float_t, DeviceType::GPU> curve,
		const nv::vec2<Float_t>* pts,
		const Float_t* tsamples, 
		const nv::vec2<Float_t>* p_tsamples,
		const nv::vec2<Float_t>* tangents,
		const nv::vec2<Float_t>* normals,
		Float_t* t_found, 
		nv::vec2<Float_t>* pts_projected,
		bool* found, Float_t* tl, Float_t* tr)  // output t_found, found, tl, tr
	{
		extern __shared__ DistTk<Float_t> sdata[]; // obtuse: [0, blocksize)  acute: [blocksize, 2*blocksize)

		int ptid = blockIdx.x;
		if (found[ptid]) return;
		int tid = threadIdx.x;
		sdata[tid] = DistTk<Float_t>();
		sdata[tid + blockDim.x] = DistTk<Float_t>();
		
		__syncthreads();

		int tk_idx = tid + ptid * blockDim.x;

		Float_t tk = tsamples[tk_idx];
		auto p_tk = p_tsamples[tk_idx];
		auto line1 = Line<nv::vec2<Float_t>>(p_tk, normals[tk_idx]);
		auto line2 = Line<nv::vec2<Float_t>>(p_tk, tangents[tk_idx]);
		Float_t alpha = 0.3;
		//Float_t alpha = 0;
		const auto& p = pts[ptid];
		auto p1 = p - p_tk;
		auto p2 = tangents[tk_idx];
		Float_t dist1 = line1.distance(p);
		Float_t dist2 = line2.distance(p);
		Float_t dist = dist1 * (1 - alpha) + dist2 * alpha;
		Float_t _dot = nv::dot(nv::normalize(p1), p2);
		if (_dot <0) {
			// obtuse angle
			sdata[tid] = DistTk<Float_t>(dist, tk);
		}
		else if (_dot >=0) {
			// acute angle
			sdata[tid + blockDim.x] = DistTk<Float_t>(dist, tk);
		}
		//else {
			// found the exact t
		//	t_found[ptid] = tk;
		//	pts_projected[ptid] = p_tk;
			//found[ptid] = true;
		//}
		__syncthreads();
		if (found[ptid]) return;

		for (int s = blockDim.x / 2; s > 0; s >>= 1) {
			if (tid < s) {
				sdata[tid].merge(sdata[tid + s]);
				//if (ptid == 1) { printf("sdata[%d]: (%f, %f)\n", tid, sdata[tid].dist, sdata[tid].tk); }
				sdata[tid + blockDim.x].merge(sdata[tid + blockDim.x + s]);
				//if (ptid == 1) { printf("sdata[%d]: (%f, %f)\n", tid + blockDim.x, sdata[tid + blockDim.x].dist, sdata[tid + blockDim.x].tk); }
			}
			__syncthreads(); //ensure all merges at one iteration are done
		}
		if (tid == 0) {
			auto &s1 = sdata[0];
			auto &s2 = sdata[blockDim.x];

			Float_t _max = NumericLimit<Float_t>::max();
			/*
			if (s1.dist == _max || s2.dist == _max) {
				Float_t tmin = curve.tMin();
				auto ptmin = curve.at(tmin);
				auto tgt_min = curve.tangent(tmin);
				Float_t dot_tmin = nv::dot(nv::normalize(p - ptmin), tgt_min);
				if (dot_tmin >= 0) {
					found[ptid] = true;
					t_found[ptid] = tmin;
					pts_projected[ptid] = ptmin;
					return;
				}
				Float_t tmax = curve.tMax() - EPS;
				auto ptmax = curve.at(tmax);
				auto tgt_max = curve.tangent(tmax);
				Float_t dot_tmax = nv::dot(nv::normalize(p - ptmax), tgt_max);
				if (dot_tmax > 0) {
					found[ptid] = false;
					tl[ptid] = s1.tk;
					tr[ptid] = tmax;
					return;
				}
			}*/
			
			
			Float_t t = (Float_t)(s2.dist * s1.tk + s1.dist * s2.tk) / (s1.dist + s2.dist);
			if (curve.tMin() > t || t >= curve.tMax()) {
				t = (tl[ptid] + tr[ptid]) / 2;
			}
			t_found[ptid] = t;
			
			// check if the new satisfies requirements
			auto pt_new = curve.at(t);
			auto tgt = curve.tangent(t);
			auto v = p - pt_new;
			Float_t dot_new = nv::dot(nv::normalize(v), tgt);
			if (dot_new <= -EPS) {
				s1.tk = t;
			}
			else if (dot_new >= EPS) {
				s2.tk = t;
			}
			else {
				//auto nk = curve.normal(t);
				//Float_t side = nv::dot(v, nk);
				//Float_t d = nv::length(v);
				//if (side < 0 || d < 1.0 / curve.curvature(t)) {
					found[ptid] = true;
					pts_projected[ptid] = pt_new;
					return;
				//}
			}
			Float_t t1 = s1.tk, t2 = s2.tk;
			if (t1 > t2) {
				t1 = t2;
				t2 = s1.tk;
			}
			//found[ptid] = false;
			if (t1 < _max) tl[ptid] = t1;
			if (t2 < _max) tr[ptid] = t2;
		}
	}

	template<typename Float_t>
	__global__ void pickClosestTksLBVH(
		CubicBSplineCurve<Float_t, DeviceType::GPU> curve,
		const int* isects, const int* bin_count,
		const nv::vec2<Float_t>* pts,
		const Float_t* tsamples, 
		const nv::vec2<Float_t>* p_tsamples,
		const nv::vec2<Float_t>* tangents,
		const nv::vec2<Float_t>* normals,
		Float_t* t_found,
		nv::vec2<Float_t>* pts_projected,
		bool* found, Float_t* tl, Float_t* tr)  // output t_found, found, tl, tr
	{
		extern __shared__ DistTk<Float_t> sdata[]; // obtuse: [0, blocksize)  acute: [blocksize, 2*blocksize)

		int ptid = blockIdx.x;
		if (found[ptid]) return;
		int tid = threadIdx.x;
		sdata[tid] = DistTk<Float_t>();
		sdata[tid + blockDim.x] = DistTk<Float_t>();
		__syncthreads();

		int cnt = bin_count[ptid];
		if (tid >= cnt) {
			return;
		}

		int tk_idx = isects[tid + ptid * blockDim.x];

		Float_t tk = tsamples[tk_idx];
		auto p_tk = p_tsamples[tk_idx];
		auto line1 = Line<nv::vec2<Float_t>>(p_tk, normals[tk_idx]);
		auto line2 = Line<nv::vec2<Float_t>>(p_tk, tangents[tk_idx]);
		Float_t alpha = 0.1;
		const auto& p = pts[ptid];
		auto p1 = p - p_tk;
		auto p2 = tangents[tk_idx];
		Float_t dist1 = line1.distance(p);
		Float_t dist2 = line2.distance(p);
		Float_t dist = dist1 * (1 - alpha) + dist2 * alpha;
		Float_t _dot = nv::dot(nv::normalize(p1), p2);
		if (_dot < 0) {
			// obtuse angle
			sdata[tid] = DistTk<Float_t>(dist, tk);
		}
		else if (_dot >= 0) {
			// acute angle
			sdata[tid + blockDim.x] = DistTk<Float_t>(dist, tk);
		}/*
		else {
			// found the exact t
			t_found[ptid] = tk;
			pts_projected[ptid] = p_tk;
			found[ptid] = true;
		}*/
		__syncthreads();
		if (found[ptid]) return;

		for (int s = blockDim.x / 2; s > 0; s >>= 1) {
			if (tid < s) {
				sdata[tid].merge(sdata[tid + s]);
				sdata[tid + blockDim.x].merge(sdata[tid + blockDim.x + s]);
			}
			__syncthreads(); //ensure all merges at one iteration are done
		}
		if (tid == 0) {
			auto& s1 = sdata[0];
			auto& s2 = sdata[blockDim.x];

			Float_t _max = NumericLimit<Float_t>::max();
			/*
			if (s1.dist < _max && s2.dist == _max) {
				Float_t tmin = curve.tMin();
				auto ptmin = curve.at(tmin);
				auto tgt_min = curve.tangent(tmin);
				Float_t dot_tmin = nv::dot(nv::normalize(p - ptmin), tgt_min);
				if (dot_tmin > 0) {
					found[ptid] = false;
					tl[ptid] = tmin;
					tr[ptid] = s1.tk;
					return;
				}
				Float_t tmax = curve.tMax();
				auto ptmax = curve.at(tmax);
				auto tgt_max = curve.tangent(tmax);
				Float_t dot_tmax = nv::dot(nv::normalize(p - ptmax), tgt_max);
				if (dot_tmax > 0) {
					found[ptid] = false;
					tl[ptid] = s1.tk;
					tr[ptid] = tmax;
					return;
				}
			}
			if (s2.dist < _max && s1.dist == _max) {
				Float_t tmin = curve.tMin();
				auto ptmin = curve.at(tmin);
				auto tgt_min = curve.tangent(tmin);
				Float_t dot_tmin = nv::dot(nv::normalize(p - ptmin), tgt_min);
				if (dot_tmin < 0) {
					found[ptid] = false;
					tl[ptid] = tmin;
					tr[ptid] = s2.tk;
					return;
				}
				Float_t tmax = curve.tMax();
				auto ptmax = curve.at(tmax);
				auto tgt_max = curve.tangent(tmax);
				Float_t dot_tmax = nv::dot(nv::normalize(p - ptmax), tgt_max);
				if (dot_tmax < 0) {
					found[ptid] = false;
					tl[ptid] = s2.tk;
					tr[ptid] = tmax;
					return;
				}
			}*/

			Float_t t = (Float_t)(s2.dist * s1.tk + s1.dist * s2.tk) / (s1.dist + s2.dist);
			if (curve.tMin() > t || t >= curve.tMax()) {
				t = (tl[ptid] + tr[ptid]) / 2;
			}
			t_found[ptid] = t;
			// check if the new satisfies requirements
			auto pt_new = curve.at(t);
			auto tgt = curve.tangent(t);
			Float_t dot_new = nv::dot(nv::normalize(p - pt_new), tgt);
			if (dot_new <= -EPS) {
				s1.tk = t;
			}
			else if (dot_new >= EPS) {
				s2.tk = t;
			}
			else {
				found[ptid] = true;
				pts_projected[ptid] = pt_new;
				return;
			}
			Float_t t1 = s1.tk, t2 = s2.tk;
			if (t1 > t2) {
				t1 = t2;
				t2 = s1.tk;
			}
			//found[ptid] = false;
			if (t1 < _max) tl[ptid] = t1;
			if (t2 < _max) tr[ptid] = t2;
		}
	}

	template<typename Float_t>
	__global__ void calcProjectionDistance2(
		int size,
		nv::vec2<Float_t>* pts,
		nv::vec2<Float_t>* pts_projected,
		Float_t *dist
	)
	{
		int idx = blockIdx.x * blockDim.x + threadIdx.x;
		if (idx >= size) return;
		dist[idx] = nv::square_norm(pts[idx] - pts_projected[idx]);
	}
}
}
}
